#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy, sys
from time import sleep
from sg805_driver.msg import FastPoint, ExtraFeatures




class FastPointDemo:

    fastPointMsg = FastPoint()
    extraMsg = ExtraFeatures()

    def __init__(self):
        rospy.init_node('fastPointCommander', anonymous = False )
        self.fastPointPub = rospy.Publisher("/sg805/fastPoint", FastPoint, queue_size=10)
        self.extraPub = rospy.Publisher("/sg805/extraFeatures", ExtraFeatures, queue_size=10)

        self.fastPointMsg.Mode = 0



        sleep(2)    # 建立连接之后不能马上发送数据



        print("下降")
        self.fastPointMsg.Position.clear()
        self.fastPointMsg.duration = 1000
        self.fastPointMsg.Position.append(3584)
        self.fastPointMsg.Position.append(36864)        # 8192
        self.fastPointMsg.Position.append(-2048)
        self.fastPointMsg.Position.append(-1024)
        self.fastPointMsg.Position.append(-8192)    # -8192
        self.fastPointMsg.Position.append(-14080)
        #self.fastPointMsg.Position.append(27136)
        self.fastPointPub.publish(self.fastPointMsg)
        sleep(1.5)

        print("抓取")
        self.grasp()


        print("悬1")
        self.fastPointMsg.Position.clear()
        self.fastPointMsg.duration = 1000
        self.fastPointMsg.Position.append(3584)        # 8192
        self.fastPointMsg.Position.append(22763)
        self.fastPointMsg.Position.append(1489)
        self.fastPointMsg.Position.append(-1024)
        self.fastPointMsg.Position.append(-13422)   # -13422
        self.fastPointMsg.Position.append(-14080)
        #self.fastPointMsg.Position.append(27136) 
        self.fastPointPub.publish(self.fastPointMsg)
        sleep(1.5)


        # print("悬1")
        # self.fastPointMsg.Position.clear()
        # self.fastPointMsg.duration = 1000
        # self.fastPointMsg.Position.append(-1024)        # 8192
        # self.fastPointMsg.Position.append(22763)
        # self.fastPointMsg.Position.append(1489)
        # self.fastPointMsg.Position.append(-1024)
        # self.fastPointMsg.Position.append(-13422)   # -13422
        # self.fastPointMsg.Position.append(-1024)
        # #self.fastPointMsg.Position.append(27136) 
        # self.fastPointPub.publish(self.fastPointMsg)
        # sleep(1.5)

        print("悬2")
        self.fastPointMsg.Position.clear()
        self.fastPointMsg.duration = 1000
        self.fastPointMsg.Position.append(-5632)        # 8192
        self.fastPointMsg.Position.append(22763)
        self.fastPointMsg.Position.append(1489)
        self.fastPointMsg.Position.append(-1024)
        self.fastPointMsg.Position.append(-13422)   # -13422
        self.fastPointMsg.Position.append(12032)
        #self.fastPointMsg.Position.append(27136) 
        self.fastPointPub.publish(self.fastPointMsg)
        sleep(1.5)



        print("下降")
        self.fastPointMsg.Position.clear()
        self.fastPointMsg.duration = 1000
        self.fastPointMsg.Position.append(-5632)
        self.fastPointMsg.Position.append(36864)        # 8192
        self.fastPointMsg.Position.append(-2048)
        self.fastPointMsg.Position.append(-1024)
        self.fastPointMsg.Position.append(-8192)    # -8192
        self.fastPointMsg.Position.append(12032)
        #self.fastPointMsg.Position.append(27136)
        self.fastPointPub.publish(self.fastPointMsg)
        sleep(1.5)

        print("释放")
        self.release()


        print("悬2")
        self.fastPointMsg.Position.clear()
        self.fastPointMsg.duration = 1000
        self.fastPointMsg.Position.append(-5632)        # 8192
        self.fastPointMsg.Position.append(22763)
        self.fastPointMsg.Position.append(1489)
        self.fastPointMsg.Position.append(-1024)
        self.fastPointMsg.Position.append(-13422)   # -13422
        self.fastPointMsg.Position.append(12032)
        #self.fastPointMsg.Position.append(27136) 
        self.fastPointPub.publish(self.fastPointMsg)
        sleep(1.5)

        print("悬0")
        self.fastPointMsg.Position.clear()
        self.fastPointMsg.duration = 4000
        self.fastPointMsg.Position.append(3584)        # 8192
        self.fastPointMsg.Position.append(22763)
        self.fastPointMsg.Position.append(1489)
        self.fastPointMsg.Position.append(-1024)
        self.fastPointMsg.Position.append(-13422)   # -13422
        self.fastPointMsg.Position.append(-14080)
        #self.fastPointMsg.Position.append(27136) 
        self.fastPointPub.publish(self.fastPointMsg)
        sleep(5)





    # 释放
    def release(self):
        self.rads = 25
        self.sleepDuration = int(self.rads*0.16) + 1
        self.extraMsg.Tag = 6
        self.extraMsg.PSC = 499
        self.extraMsg.ARR = 9
        self.extraMsg.CCR1 = 5
        self.extraMsg.PluseCount = 6400 * self.rads        # 0.16秒/转
        self.extraPub.publish(self.extraMsg)
        sleep(self.sleepDuration)


    # 夹取
    def grasp(self):
        self.rads = 25
        self.sleepDuration = int(self.rads*0.16) + 1
        self.extraMsg.Tag = 6
        self.extraMsg.PSC = 499
        self.extraMsg.ARR = 9
        self.extraMsg.CCR1 = 5
        self.extraMsg.PluseCount = -6400 * self.rads   # 0.16秒/转
        self.extraPub.publish(self.extraMsg)
        sleep(self.sleepDuration)


if __name__ == '__main__':
    try:
        handle = FastPointDemo()
    except rospy.ROSInterruptException:
        pass